#include "vsf_err.h"

#include "app_cfg.h"
#include "app_type.h"

#include "interfaces_cfg.h"
#include "interfaces_const.h"
#include "core.h"

#include "M451Series.h"

#define M45X_CLK_PLLCTL_NR(x)				(((x) - 2) << 9)
#define M45X_CLK_PLLCTL_NF(x)				(((x) - 2) << 0)

#define M45X_CLK_PLLCTL_NO_1				(0x0UL << CLK_PLLCTL_OUTDIV_Pos)
#define M45X_CLK_PLLCTL_NO_2				(0x1UL << CLK_PLLCTL_OUTDIV_Pos)
#define M45X_CLK_PLLCTL_NO_4				(0x3UL << CLK_PLLCTL_OUTDIV_Pos)

#define M45X_CLK_CLKSEL0_HCLKSEL_HXT		(0x00UL << CLK_CLKSEL0_HCLKSEL_Pos)
#define M45X_CLK_CLKSEL0_HCLKSEL_LXT		(0x01UL << CLK_CLKSEL0_HCLKSEL_Pos)
#define M45X_CLK_CLKSEL0_HCLKSEL_PLL		(0x02UL << CLK_CLKSEL0_HCLKSEL_Pos)
#define M45X_CLK_CLKSEL0_HCLKSEL_LIRC		(0x03UL << CLK_CLKSEL0_HCLKSEL_Pos)
#define M45X_CLK_CLKSEL0_HCLKSEL_HIRC		(0x07UL << CLK_CLKSEL0_HCLKSEL_Pos)

static struct m45x_info_t m45x_info =
{
	0, CORE_VECTOR_TABLE,
	CORE_CLKEN, CORE_HCLKSRC, CORE_PCLKSRC, CORE_PLLSRC,
	OSC0_FREQ_HZ, OSC32_FREQ_HZ, 0, 0,
	CORE_PLL_FREQ_HZ, CPU_FREQ_HZ, HCLK_FREQ_HZ, PCLK_FREQ_HZ,
};

struct m45x_info_t *m45x_interface_get_info(void)
{
	return &m45x_info;
}

vsf_err_t m45x_interface_fini(void *p)
{
	return VSFERR_NONE;
}

vsf_err_t m45x_interface_reset(void *p)
{
	return VSFERR_NONE;
}

uint32_t m45x_interface_get_stack(void)
{
	return __get_MSP();
}

vsf_err_t m45x_interface_set_stack(uint32_t sp)
{
	__set_MSP(sp);
	return VSFERR_NONE;
}

// sleep will enable interrupt
// for cortex processor, if an interrupt occur between enable the interrupt
// 		and __WFI, wfi will not make the core sleep
void m45x_interface_sleep(uint32_t mode)
{
	vsf_leave_critical();
	__WFI();
}

void m45x_unlock_reg(void)
{
	while((SYS->REGLCTL & 0x01) != 0x01)
	{
        SYS->REGLCTL = 0x59;
        SYS->REGLCTL = 0x16;
        SYS->REGLCTL = 0x88;
    }
}

void m45x_lock_reg(void)
{
	SYS->REGLCTL = 0;
}

vsf_err_t m45x_interface_init(void *p)
{
	uint32_t temp32;
	uint32_t freq_in;

	if (p != NULL)
	{
		m45x_info = *(struct m45x_info_t *)p;
	}
	m45x_info.hirc_freq_hz = 22 * 1000 * 1000;
	m45x_info.lirc_freq_hz = 10 * 1000;

	m45x_unlock_reg();

	if (m45x_info.clk_enable & M45X_CLK_HXT)
	{
		CLK->PWRCTL |= CLK_PWRCTL_HXTGAIN_Msk;
		if (m45x_info.osc_freq_hz <= 8 * 1000 * 1000)
		{
			CLK->PWRCTL &= ~(0x3ul << CLK_PWRCTL_HXTGAIN_Pos);
		}
		else if (m45x_info.osc_freq_hz <= 12 * 1000 * 1000)
		{
			CLK->PWRCTL &= ~(0x2ul << CLK_PWRCTL_HXTGAIN_Pos);
		}
		else if (m45x_info.osc_freq_hz <= 16 * 1000 * 1000)
		{
			CLK->PWRCTL &= ~(0x1ul << CLK_PWRCTL_HXTGAIN_Pos);
		}
	}

	CLK->PWRCTL |= m45x_info.clk_enable;
	temp32 = m45x_info.clk_enable;
	temp32 = ((temp32 & M45X_CLK_HXT) ? CLK_STATUS_HXTSTB_Msk : 0) |
				((temp32 & M45X_CLK_LXT) ? CLK_STATUS_LXTSTB_Msk : 0) |
				((temp32 & M45X_CLK_HIRC) ? CLK_STATUS_HIRCSTB_Msk : 0) |
				((temp32 & M45X_CLK_LIRC) ? CLK_STATUS_LIRCSTB_Msk : 0);
	while ((CLK->STATUS & temp32) != temp32);

	// enable PLLs
	if (m45x_info.pllsrc != M45X_PLLSRC_NONE)
	{
		uint8_t no;
		uint32_t no_mask;

		switch (m45x_info.pllsrc)
		{
		case M45X_PLLSRC_HXT:
			temp32 = 0;
			freq_in = m45x_info.osc_freq_hz;
			break;
		case M45X_PLLSRC_HIRC:
			temp32 = CLK_PLLCTL_PLLSRC_Msk;
			freq_in = m45x_info.hirc_freq_hz;
			break;
		default:
			return VSFERR_INVALID_PARAMETER;
		}

		if (freq_in > 12 * 1000 * 1000)
		{
			temp32 |= CLK_PLLCTL_STBSEL_Msk;
		}

		// Fin/NR: 2MHz
		if ((m45x_info.pll_freq_hz * 1 >= (250 * 1000 * 1000)) &&
				(m45x_info.pll_freq_hz * 1 < (500 * 1000 * 1000)))
		{
			no = 1;
			no_mask = M45X_CLK_PLLCTL_NO_1;
		}
		else if ((m45x_info.pll_freq_hz * 2 >= (250 * 1000 * 1000)) &&
				(m45x_info.pll_freq_hz * 2 < (500 * 1000 * 1000)))
		{
			no = 2;
			no_mask = M45X_CLK_PLLCTL_NO_2;
		}
		else if ((m45x_info.pll_freq_hz * 4 >= (250 * 1000 * 1000)) &&
				(m45x_info.pll_freq_hz * 4 < (500 * 1000 * 1000)))
		{
			no = 4;
			no_mask = M45X_CLK_PLLCTL_NO_4;
		}
		else
		{
			return VSFERR_INVALID_PARAMETER;
		}

		CLK->PLLCTL = temp32 | no_mask |
			M45X_CLK_PLLCTL_NR(freq_in / 2000000) |
			M45X_CLK_PLLCTL_NF(m45x_info.pll_freq_hz * no / 2000000);
		while ((CLK->STATUS & CLK_STATUS_PLLSTB_Msk) != CLK_STATUS_PLLSTB_Msk);
	}

	// set pclk
	if (m45x_info.pclksrc == M45X_PCLKSRC_HCLKd2)
	{
		CLK->CLKSEL0 |= CLK_CLKSEL0_PCLK0SEL_Msk | CLK_CLKSEL0_PCLK1SEL_Msk;
	}
	else
	{
		CLK->CLKSEL0 &= ~(CLK_CLKSEL0_PCLK0SEL_Msk | CLK_CLKSEL0_PCLK1SEL_Msk);
	}

	// set hclk
	switch (m45x_info.hclksrc)
	{
	case M45X_HCLKSRC_HIRC:
		freq_in = m45x_info.hirc_freq_hz;
		temp32 = M45X_CLK_CLKSEL0_HCLKSEL_HIRC;
		break;
	case M45X_HCLKSRC_LIRC:
		freq_in = m45x_info.lirc_freq_hz;
		temp32 = M45X_CLK_CLKSEL0_HCLKSEL_LIRC;
		break;
	case M45X_HCLKSRC_PLLFOUT:
		freq_in = m45x_info.pll_freq_hz;
		temp32 = M45X_CLK_CLKSEL0_HCLKSEL_PLL;
		break;
	case M45X_HCLKSRC_LXT:
		freq_in = m45x_info.osc32k_freq_hz;
		temp32 = M45X_CLK_CLKSEL0_HCLKSEL_LXT;
		break;
	case M45X_HCLKSRC_HXT:
		freq_in = m45x_info.osc_freq_hz;
		temp32 = M45X_CLK_CLKSEL0_HCLKSEL_HXT;
		break;
	default:
		return VSFERR_INVALID_PARAMETER;
	}
	CLK->CLKDIV0 = (CLK->CLKDIV0 & ~CLK_CLKDIV0_HCLKDIV_Msk) | 15;
	CLK->CLKSEL0 = (CLK->CLKSEL0 & ~CLK_CLKSEL0_HCLKSEL_Msk) | temp32;
	CLK->CLKDIV0 = (CLK->CLKDIV0 & ~CLK_CLKDIV0_HCLKDIV_Msk) |
			((freq_in / m45x_info.hclk_freq_hz) - 1);



	// close unused clock
	/*
	if (!(m45x_info.clk_enable & M45X_CLK_HXT))
		CLK->PWRCTL &= ~CLK_PWRCTL_HXTEN_Msk;
	if (!(m45x_info.clk_enable & M45X_CLK_LXT))
		CLK->PWRCTL &= ~CLK_PWRCTL_LXTEN_Msk;
	if (!(m45x_info.clk_enable & M45X_CLK_HIRC))
		CLK->PWRCTL &= ~CLK_PWRCTL_HIRCEN_Msk;
	if (!(m45x_info.clk_enable & M45X_CLK_LIRC))
		CLK->PWRCTL &= ~CLK_PWRCTL_LIRCEN_Msk;
	*/

	m45x_lock_reg();

	SCB->VTOR = m45x_info.vector_table;
	SCB->AIRCR = 0x05FA0000 | m45x_info.priority_group;
	return VSFERR_NONE;
}

uint32_t m45x_uid_get(uint8_t *buffer, uint32_t size)
{
	return 0;
}

#define SYSTICK_ENABLE				(1 << 0)
#define SYSTICK_CLKSOURCE			(1 << 2)
#define SYSTICK_COUNTFLAG			(1 << 16)

vsf_err_t m45x_delay_init(void)
{
	SysTick->CTRL = SYSTICK_CLKSOURCE;
	SysTick->VAL = 0;
	return VSFERR_NONE;
}

static vsf_err_t m45x_delay_delayus_do(uint32_t tick)
{
	uint32_t dly_tmp;

	m45x_delay_init();
	while (tick)
	{
		dly_tmp = (tick > ((1 << 24) - 1)) ? ((1 << 24) - 1) : tick;
		SysTick->LOAD = dly_tmp;
		SysTick->CTRL |= SYSTICK_ENABLE;
		while (!(SysTick->CTRL & SYSTICK_COUNTFLAG));
		m45x_delay_init();
		tick -= dly_tmp;
	}
	return VSFERR_NONE;
}

vsf_err_t m45x_delay_delayus(uint16_t us)
{
	m45x_delay_delayus_do(us * (m45x_info.cpu_freq_hz / (1000 * 1000)));
	return VSFERR_NONE;
}

vsf_err_t m45x_delay_delayms(uint16_t ms)
{
	m45x_delay_delayus_do(ms * (m45x_info.cpu_freq_hz / 1000));
	return VSFERR_NONE;
}

static void (*m45x_tickclk_callback)(void *param) = NULL;
static void *m45x_tickclk_param = NULL;
static volatile uint32_t m45x_tickcnt = 0;

void TMR3_IRQHandler(void)
{
#if 0
	uint32_t last_interval;
	last_interval = TIMER3->CMP / (m45x_info.lirc_freq_hz / 1000);
	m45x_tickcnt += last_interval;
	TIMER3->CMP = m45x_info.lirc_freq_hz / 1000;
	if (m45x_tickclk_callback != NULL)
	{
		m45x_tickclk_callback(m45x_tickclk_param);
	}
	TIMER3->INTSTS = TIMER_INTSTS_TIF_Msk;
#endif

	m45x_tickcnt++;
	if (m45x_tickclk_callback != NULL)
	{
		m45x_tickclk_callback(m45x_tickclk_param);
	}
	TIMER3->INTSTS = TIMER_INTSTS_TIF_Msk;
}

vsf_err_t m45x_tickclk_start(void)
{
	TIMER3->CTL |= TIMER_CTL_CNTEN_Msk;
	return VSFERR_NONE;
}

vsf_err_t m45x_tickclk_stop(void)
{
	TIMER3->CTL &= ~TIMER_CTL_CNTEN_Msk;
	return VSFERR_NONE;
}

vsf_err_t m45x_tickclk_set_interval(uint32_t interval)
{
#if 0
	uint32_t cmpdat, last_interval;
	last_interval = TIMER3->CMP / (m45x_info.lirc_freq_hz / 1000);
	if (interval > last_interval)
	{
		cmpdat = m45x_info.lirc_freq_hz / 1000 * interval;
		if (interval >= 0x1ul << 24)
		{
			return VSFERR_FAIL;
		}
		if (TIMER3->CTL & TIMER_CTL_CNTEN_Msk)
		{
			TIMER3->CTL &= ~TIMER_CTL_CNTEN_Msk;
			TIMER3->CMP = cmpdat;
			TIMER3->CTL |= TIMER_CTL_CNTEN_Msk;
		}
		else
		{
			TIMER3->CMP = cmpdat;
		}
	}
	else if (interval < last_interval)
	{
		return VSFERR_FAIL;
	}
	return VSFERR_NONE;
#endif

	if (interval >= 0x1ul << 24)
	{
		return VSFERR_FAIL;
	}
	if (TIMER3->CTL & TIMER_CTL_CNTEN_Msk)
	{
		TIMER3->CTL &= ~TIMER_CTL_CNTEN_Msk;
		TIMER3->CMP = m45x_info.pclk_freq_hz / 1000 * interval;
		TIMER3->CTL |= TIMER_CTL_CNTEN_Msk;
	}
	else
	{
		TIMER3->CMP = m45x_info.pclk_freq_hz / 1000 * interval;
	}
	return VSFERR_NONE;
}

static uint32_t m45x_tickclk_get_count_local(void)
{
	return m45x_tickcnt;
}

uint32_t m45x_tickclk_get_count(void)
{
	uint32_t count1, count2;

	do {
		count1 = m45x_tickclk_get_count_local();
		count2 = m45x_tickclk_get_count_local();
	} while (count1 != count2);
	return count1;
}

vsf_err_t m45x_tickclk_set_callback(void (*callback)(void*), void *param)
{
	if (TIMER3->CTL & TIMER_CTL_CNTEN_Msk)
	{
		TIMER3->CTL &= ~TIMER_CTL_CNTEN_Msk;
		m45x_tickclk_callback = callback;
		m45x_tickclk_param = param;
		TIMER3->CTL |= TIMER_CTL_CNTEN_Msk;
	}
	else
	{
		m45x_tickclk_callback = callback;
		m45x_tickclk_param = param;
	}
	return VSFERR_NONE;
}

vsf_err_t m45x_tickclk_init(void)
{
	m45x_tickcnt = 0;
	CLK->CLKSEL1 &= ~CLK_CLKSEL1_TMR3SEL_Msk;
	CLK->CLKSEL1 |= 0x2 << CLK_CLKSEL1_TMR3SEL_Pos;
	CLK->APBCLK0 |= CLK_APBCLK0_TMR3CKEN_Msk;
	TIMER3->CTL = TIMER_CTL_RSTCNT_Msk;
	TIMER3->CTL = TIMER_CTL_INTEN_Msk | (0x1ul << TIMER_CTL_OPMODE_Pos) |
					TIMER_CTL_WKEN_Msk;
	m45x_tickclk_set_interval(1);
	NVIC_EnableIRQ(TMR3_IRQn);
	return VSFERR_NONE;
}

vsf_err_t m45x_tickclk_fini(void)
{
	TIMER3->CTL = 0;
	CLK->APBCLK0 &= ~CLK_APBCLK0_TMR3CKEN_Msk;
	return VSFERR_NONE;
}

